﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Research.Kinect.Nui;
using Nui = Microsoft.Research.Kinect.Nui;

namespace MSUS.Kinect.SkeletonTracker.Utils
{
	public static class VectorExtensions
	{
		public static Vector ScaleToScreen(this Vector vector, int maxPixelX, int maxPixelY, float maxRawX, float maxRawY)
		{
			Vector scaled = new Vector()
			{
				X = Scale(maxPixelX, maxRawX, vector.X),
				Y = Scale(maxPixelY, maxRawY, -vector.Y),
				Z = vector.Z,
				W = vector.W
			};

			return scaled;
		}

		const float projectionPlaneDist = 2.0f;
		public static Vector ScaleToScreenAccurate(this Vector vector, int maxPixelX, int maxPixelY, float maxRawX, float maxRawY)
		{
			if (vector.Z == 0)	//	no depth info, so default to simple calc
			{
				return ScaleToScreen(vector, maxPixelX, maxPixelY, maxRawX, maxRawY);
			}
			else
			{
				float projectedZ = (projectionPlaneDist / vector.Z);
				float projectedX = vector.X * projectedZ;
				float projectedY = -vector.Y * projectedZ;
				Vector scaled = new Vector()
				{
					X = Scale(maxPixelX, maxRawX, projectedX),
					Y = Scale(maxPixelY, maxRawY, projectedY),
					Z = vector.Z,
					W = vector.W
				};
				return scaled;
			}
		}

		public static Vector ScaleToScreen(this Vector vector, int maxPixelX, int maxPixelY)
		{
			return ScaleToScreen(vector, maxPixelX, maxPixelY, 1.0f, 1.0f);
		}

		private static float Scale(int maxPixel, float maxSkeleton, float position)
		{
			float value = ((((maxPixel / maxSkeleton) / 2) * position) + (maxPixel / 2));
			if (value > maxPixel)
				return maxPixel;
			if (value < 0)
				return 0;
			return value;
		}


		public static float GetDist(this Vector vectorA, Vector vectorB)
		{
			float x, y, z;
			x = vectorA.X - vectorB.X;
			y = vectorA.Y - vectorB.Y;
			z = vectorA.Z - vectorB.Z;

			double dist1 = Math.Sqrt((x * x) + (y * y));
			double dist2 = Math.Sqrt((dist1 * dist1) + (z * z));
			return (float)dist2;
		}
	}
}
